#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <cmath>
#include <limits>
#include <algorithm>
#include <memory>

// 定义地图大小
const int MAP_WIDTH = 10;
const int MAP_HEIGHT = 10;

// 定义地图，1表示障碍物，0表示可通过
int map[MAP_HEIGHT][MAP_WIDTH] = {
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 1, 1, 1, 1, 1, 1, 1, 1, 0},
    {0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
    {0, 1, 0, 1, 1, 1, 1, 1, 1, 0},
    {0, 1, 0, 1, 0, 0, 0, 0, 0, 0},
    {0, 1, 0, 1, 0, 1, 1, 1, 1, 0},
    {0, 1, 0, 1, 0, 1, 0, 0, 0, 0},
    {0, 1, 0, 1, 0, 1, 0, 1, 1, 0},
    {0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
    {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};

// 定义节点结构体，使用std::shared_ptr管理父节点指针
struct Node
{
    int x, y;
    int g;
    std::shared_ptr<Node> parent;

    Node(int x, int y, std::shared_ptr<Node> parent, int g)
        : x(x), y(y), parent(parent), g(g) {}
};

// 比较节点的仿函数结构体，用于优先队列排序
struct CompareNodes
{
    bool operator()(const std::shared_ptr<Node> &a, const std::shared_ptr<Node> &b)
    {
        return a->g > b->g;
    }
};

// 重构路径函数，返回从起始节点到目标节点的路径
std::vector<std::shared_ptr<Node>> reconstruct_path(std::shared_ptr<Node> current)
{
    std::vector<std::shared_ptr<Node>> total_path;
    while (current != nullptr)
    {
        total_path.push_back(current);
        current = current->parent;
    }
    std::reverse(total_path.begin(), total_path.end());
    return total_path;
}

// Dijkstra算法核心函数
std::vector<std::shared_ptr<Node>> dijkstra(
    const std::shared_ptr<Node> &start,
    const std::shared_ptr<Node> &goal)
{
    if (start == goal)
    {
        return {start};
    }
    if (map[start->x][start->y] == 1)
    {
        std::cout << "start is obstacle" << std::endl;
        return {};
    }
    if (map[goal->x][goal->y] == 1)
    {
        std::cout << "goal is obstacle" << std::endl;
        return {};
    }
    std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>, CompareNodes> open_set;
    std::unordered_map<int, std::shared_ptr<Node>> openListMap;
    std::unordered_map<int, std::shared_ptr<Node>> closeListMap;
    open_set.push(start);
    openListMap[start->x * MAP_HEIGHT + start->y] = start;

    while (!open_set.empty())
    {
        auto current = open_set.top();
        open_set.pop();
        openListMap.erase(current->x * MAP_HEIGHT + current->y);

        if (current->x == goal->x && current->y == goal->y)
        {
            std::cout << "Found path" << std::endl;
            return reconstruct_path(current);
        }
        closeListMap[current->x * MAP_HEIGHT + current->y] = current;
        for (int dx = -1; dx <= 1; dx++)
        {
            for (int dy = -1; dy <= 1; dy++)
            {
                if (dx == 0 && dy == 0)
                {
                    continue;
                }
                if (dx == dy)
                {
                    continue;
                }
                int neighborX = current->x + dx;
                int neighborY = current->y + dy;
                // 超出地图范围
                if (neighborX < 0 || neighborX >= MAP_WIDTH || neighborY < 0 || neighborY >= MAP_HEIGHT)
                {
                    continue;
                }
                if (map[neighborX][neighborY] == 1)
                {
                    continue;
                }
                int neighborKey = neighborX * MAP_HEIGHT + neighborY;
                if (closeListMap.find(neighborKey) != closeListMap.end())
                {
                    continue;
                }
                int gCost = current->g + std::sqrt(dx * dx + dy * dy);
                auto neighborNode = openListMap[neighborKey];
                // 如果邻居节点不在待评估集合中，则将其加入
                if (neighborNode == nullptr)
                {
                    neighborNode = std::make_shared<Node>(neighborX, neighborY, current, gCost);
                    open_set.push(neighborNode);
                    openListMap[neighborKey] = neighborNode;
                }
                // 如果邻居节点已经在待评估集合中，检查是否找到了更优路径
                else if (gCost < neighborNode->g)
                {
                    neighborNode->g = gCost;
                    neighborNode->parent = current;
                    // 更新优先队列中的节点
                    open_set.push(neighborNode);
                }
            }
        }
    }

    return {}; // No path found
}

int main()
{
    auto start = std::make_shared<Node>(0, 0, nullptr, 0);
    auto goal = std::make_shared<Node>(9, 9, nullptr, 0);
    std::cout << "start=(" << start->x << "," << start->y << "), goal=(" << goal->x << "," << goal->y << ")" << std::endl;
    std::vector<std::shared_ptr<Node>> path = dijkstra(start, goal);

    if (!path.empty())
    {
        std::cout << "Path found:" << std::endl;
        for (const auto &node : path)
        {
            std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
        }
    }
    else
    {
        std::cout << "No path found." << std::endl;
    }

    return 0;
}